% validate the relative pose from absolute pose
clc;
close all;

if ~exist('data_root')
    data_root = '/media/bingo/SSD/multi_lidar_calib_data/prescan/scene_motion_plane_sync';
end

addpath('../interface');
addpath('../func');
addpath('../thirdParty/yaml');

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if ~exist('calib_gt_path')
  calib_gt_path = 'calib_gt';
end

% load the absolute pose
% [roll pitch yaw x y z] [rad rad rad meter meter meter]
vehicle_to_lidar_front = loadPoseYaml(sprintf('%s/lidar_front_pose_relative.txt', data_root));
vehicle_to_lidar_left = loadPoseYaml(sprintf('%s/lidar_left_pose_relative.txt', data_root));
vehicle_to_lidar_right = loadPoseYaml(sprintf('%s/lidar_right_pose_relative.txt', data_root));
vehicle_to_lidar_back = loadPoseYaml(sprintf('%s/lidar_back_pose_relative.txt', data_root));

% load the relative pose
% [roll pitch yaw x y z] [rad rad rad meter meter meter]
lidar_left_to_lidar_front = loadPoseYaml(sprintf('%s/%s/lidar_1_to_2_gt_6dof.txt', data_root, calib_gt_path));
lidar_right_to_lidar_front = loadPoseYaml(sprintf('%s/%s/lidar_3_to_2_gt_6dof.txt', data_root, calib_gt_path));
lidar_back_to_lidar_front = loadPoseYaml(sprintf('%s/%s/lidar_4_to_2_gt_6dof.txt', data_root, calib_gt_path));

T_vehicle2front = eye(4);
R_vehicle2front = eul2rotm([vehicle_to_lidar_front(3) vehicle_to_lidar_front(2) vehicle_to_lidar_front(1)]);
V_vehicle2front = [vehicle_to_lidar_front(4) vehicle_to_lidar_front(5) vehicle_to_lidar_front(6)]';
T_vehicle2front(1:3, 1:3) = R_vehicle2front;
T_vehicle2front(1:3, 4) = V_vehicle2front;

T_vehicle2left = eye(4);
R_vehicle2left = eul2rotm([vehicle_to_lidar_left(3) vehicle_to_lidar_left(2) vehicle_to_lidar_left(1)]);
V_vehicle2left = [vehicle_to_lidar_left(4) vehicle_to_lidar_left(5) vehicle_to_lidar_left(6)]';
T_vehicle2left(1:3, 1:3) = R_vehicle2left;
T_vehicle2left(1:3, 4) = V_vehicle2left;

T_vehicle2right = eye(4);
R_vehicle2right = eul2rotm([vehicle_to_lidar_right(3) vehicle_to_lidar_right(2) vehicle_to_lidar_right(1)]);
V_vehicle2right = [vehicle_to_lidar_right(4) vehicle_to_lidar_right(5) vehicle_to_lidar_right(6)]';
T_vehicle2right(1:3, 1:3) = R_vehicle2right;
T_vehicle2right(1:3, 4) = V_vehicle2right;

T_vehicle2back = eye(4);
R_vehicle2back = eul2rotm([vehicle_to_lidar_back(3) vehicle_to_lidar_back(2) vehicle_to_lidar_back(1)]);
V_vehicle2back = [vehicle_to_lidar_back(4) vehicle_to_lidar_back(5) vehicle_to_lidar_back(6)]';
T_vehicle2back(1:3, 1:3) = R_vehicle2back;
T_vehicle2back(1:3, 4) = V_vehicle2back;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

T_left2front = inv(T_vehicle2front) * T_vehicle2left;
eul_left2front = rotm2eul(T_left2front(1:3, 1:3)) * 180 / pi;
fprintf('left2front gt_rpyxyz: [%.4f, %.4f, %.4f, %.4f, %.4f, %.4f]\n', ...
        lidar_left_to_lidar_front(1) * 180 / pi, ...
        lidar_left_to_lidar_front(2) * 180 / pi, ...
        lidar_left_to_lidar_front(3) * 180 / pi, ...
        lidar_left_to_lidar_front(4), ...
        lidar_left_to_lidar_front(5), ...
        lidar_left_to_lidar_front(6));
fprintf('left2front est_rpyxyz: [%.4f, %.4f, %.4f, %.4f, %.4f, %.4f]\n', ...
        eul_left2front(3), ...
        eul_left2front(2), ...
        eul_left2front(1), ...
        T_left2front(1, 4), ...
        T_left2front(2, 4), ...
        T_left2front(3, 4));

T_right2front = inv(T_vehicle2front) * T_vehicle2right;
eul_right2front = rotm2eul(T_right2front(1:3, 1:3)) * 180 / pi;
fprintf('right2front gt_rpyxyz: [%.4f, %.4f, %.4f, %.4f, %.4f, %.4f]\n', ...
        lidar_right_to_lidar_front(1) * 180 / pi, ...
        lidar_right_to_lidar_front(2) * 180 / pi, ...
        lidar_right_to_lidar_front(3) * 180 / pi, ...
        lidar_right_to_lidar_front(4), ...
        lidar_right_to_lidar_front(5), ...
        lidar_right_to_lidar_front(6));
fprintf('right2front est_rpyxyz: [%.4f, %.4f, %.4f, %.4f, %.4f, %.4f]\n', ...
        eul_right2front(3), ...
        eul_right2front(2), ...
        eul_right2front(1), ...
        T_right2front(1, 4), ...
        T_right2front(2, 4), ...
        T_right2front(3, 4));

T_back2front = inv(T_vehicle2front) * T_vehicle2back;
eul_back2front = rotm2eul(T_back2front(1:3, 1:3)) * 180 / pi;
fprintf('back2front gt_rpyxyz: [%.4f, %.4f, %.4f, %.4f, %.4f, %.4f]\n', ...
        lidar_back_to_lidar_front(1) * 180 / pi, ...
        lidar_back_to_lidar_front(2) * 180 / pi, ...
        lidar_back_to_lidar_front(3) * 180 / pi, ...
        lidar_back_to_lidar_front(4), ...
        lidar_back_to_lidar_front(5), ...
        lidar_back_to_lidar_front(6));
fprintf('back2front est_rpyxyz: [%.4f, %.4f, %.4f, %.4f, %.4f, %.4f]\n', ...
        eul_back2front(3), ...
        eul_back2front(2), ...
        eul_back2front(1), ...
        T_back2front(1, 4), ...
        T_back2front(2, 4), ...
        T_back2front(3, 4));


